#include <buola/cv/io/ckinect.h>
#include <buola/cv/ccloud.h>
#include <buola/scene.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/app/ceventloop.h>
#include <buola/image/io.h>
#include <buola/image/cgc.h>
#include <buola/image/algorithm/filter.h>
#include <buola/app/ccmdline.h>
#include <buola/app/capp.h>
#include <buola/scene/cscene.h>
#include <buola/scene/cimagerenderer.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/gui/ctoolkit.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/scene/controllers/ccameracontroller.h>

using namespace buola;

void OnCloud(const img::CImage_rgb8 &pRGB,const img::CImage_gray32f &pDepth,
             scene::PPointCloud pSceneCloud,scene::CSceneView &pView)
{
    img::CImage_gray32f lFilteredDepth(pDepth.Size());
    img::bilateral_filter(pDepth,lFilteredDepth,5,0.1);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    auto lCloud=cvi::make_cloud_with_normals(pDepth,pRGB,lParams,3);
    
    {
        msg_info() << "got one!\n";
        lock_guard<recursive_mutex> lLock(get_main_event_loop().GetMutex());
        pSceneCloud->SetCloud(std::move(lCloud));
        pView.Refresh();
    }
}

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    try
    {
        CApp lApp;
        
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();

        //create the scene
        scene::PScene lScene=new scene::CScene;
        
        scene::PCube lCube=new scene::CCube;
        scene::PRTTransform lTransform=new scene::CRTTransform(mat::CQuat_d(),{0,0,5});
        lCube->AttachTo(lTransform);
        lScene->World()->AddChild(lTransform);

        scene::PPointCloud lCloud=new scene::CPointCloud();
        lScene->World()->AddObject(lCloud);

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1,50.0);
        lCamera->LookAt({0,0,1},{0,0,0},{0,-1,0});

        scene::PSceneView lView=new scene::CSceneView;
        lView->SetCamera(lCamera);
        lView->AddScene(lScene);
        lView->AddController(new scene::CCameraController(lCamera));
        lView->Map(CSize_d(800,600));
        
        cvi::CKinect lKinect;
        lKinect.sImages.Connect([&](const img::CImage_rgb8 &pRGB,const img::CImage_gray32f &pDepth){   OnCloud(pRGB,pDepth,
                                                                                                    lCloud,*lView);  });
        msg_info() << "getting one!\n";
        auto lImage=lKinect.GetOne();
        msg_info() << "saving\n";
        msg_info() << "got one 2!\n";
        lKinect.Start();
        
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
